#include "cv.h"

#define POW2(x) ((x)*(x))

/*! creating my own resize algorithm "neighbor to nearest weighted average"
*/
void resize_nnwa( const IplImage* src, IplImage* dst )
{
	assert( src->nChannels == 1 );
	assert( dst->nChannels == 1 );

	for( int y = 0; y < dst->height; y++ )
	{
		unsigned char* dr = &CV_IMAGE_ELEM( dst, unsigned char, y, 0 );
		for( int x = 0; x < dst->width; x++ )
		{
			float cy = ((float)y+.5f) * src->height / dst->height;
			float cx = ((float)x+.5f) * src->width / dst->width;

			// calculate the indices of the neighbors in the smaller src image
#define N 2
			int sy[4]; // neighboring src y
			int sx[4]; // neighboring src x
			sy[0] = max(0,				cvFloor( cy-.5f ) );
			sy[1] = min(src->height-1,	cvCeil(  cy-.5f ) );
			sy[2] = max(0,				sy[0]-1 );
			sy[3] = min(src->height-1,	sy[0]+1 );

			sx[0] = max(0,				cvFloor( cx-.5f ) );
			sx[1] = min(src->width-1,	cvCeil(  cx-.5f ) );
			sx[2] = max(0,				sx[0]-1 );
			sx[3] = min(src->width-1,	sx[0]+1 );

			// calculate the "center" of the pixels
			float fsy[N], fsx[N];
			for( int i = 0; i < N; i++ )
			{
				fsx[i] = sx[i]+.5f;
				fsy[i] = sy[i]+.5f;
			}

			// calculate the average for the neighbors
			float avg = 0.f;
			float tw = 0.f;
			for( int yi = 0; yi < N; yi++ )
			{
				for( int xi=0; xi < N; xi++ )
				{
					float w = 1.f / (.2f + 
						sqrt( 
							POW2( (cx-fsx[xi])*dst->width/src->width ) + 
							POW2( (cy-fsy[yi])*dst->height/src->height ) ));
					tw += w;
					avg += w * CV_IMAGE_ELEM( src, unsigned char, sy[yi], sx[xi] );
				}
			}
			avg /= tw;

			// find the neighbor that is the closest to the avg
			unsigned char bestColor;
			float bestDist = 255.f;
			for( int yi = 0; yi < 2; yi++ )
			{
				for( int xi=0; xi < 2; xi++ )
				{
					float dist = fabs( avg - (float)CV_IMAGE_ELEM( src, unsigned char, sy[yi], sx[xi] ) );
					if( dist < bestDist )
					{
						bestColor = CV_IMAGE_ELEM( src, unsigned char, sy[yi], sx[xi] );
						bestDist = dist;
					}
				}
			}
			CV_IMAGE_ELEM( dst, unsigned char, y, x ) = bestColor;
		}
	}
}